%% Saturn
warning off
clc;clear;
format longg
options_negx = odeset('RelTol',1e-13,'AbsTol',1e-13,'Events',@NegXcrossing);
options=odeset('RelTol',1e-13,'AbsTol',1e-13);
options_fmincon = optimoptions('fmincon', 'MaxFunctionEvaluations', 20000, ...
    'MaxIterations', 20000,'ConstraintTolerance',1e-6,"Algorithm","interior-point",...
    "EnableFeasibilityMode",true,...
    "SubproblemAlgorithm","cg");
%% Inputs.
G           = 6.674e-11 * ((1/1000)^3); % Gravitational parameters

mass_central = 1.989e30;
%[] Mass of central planet

mass_moon = 5.972e24;         %Callisto
%[] mass of moons

DU = 151.73e6;           %Callisto
%[km] Semi-major axis of Moons, used as distance units for each CRTBP
%environment.

moonName = {'Luna'};
%[] Name of the Moons


thetao = 0;
%[] Initial position of the moon relative to the first point of aries

GM_central = mass_central*G;
%[] Gravitational parameters

GM_moon = mass_moon*G;
%[] Gravitational parameters

N = length(mass_moon);
%[] Number of Moons

u = zeros(N,1);
for ii = 1:N
    u(ii) = GM_moon(ii)/(GM_moon(ii)+GM_central);
end
%[] Gravataional ratio

TU = zeros(N,1);
VU = zeros(N,1);
for ii = 1:N
    TU(ii) = sqrt(DU(ii)^3/GM_central);
    VU(ii) = DU(ii)/TU(ii);
end
%[] Time constant for each crtbp system

theta_dot = zeros(1,N);
for ii = 1:length(theta_dot)
    theta_dot(ii) = sqrt(GM_central*DU(ii))/DU(ii)^2;
end
%[] Theta dot for each planet.

planetNumb = 1;
[L1,L2,L3,L4,L5] = librationPoints(u(planetNumb));
L_ = [L1,L2,L3,L4,L5];
for ii = 1:length(L_)
    L_pos = L_(:,ii);
    J_L(ii) = jacobiConst(L_pos,zeros(3,1),u(planetNumb));
end

%%
OpenCRTBP_u(u)
%[] open plot

load('SE_L4_L5_Lyapunov.mat');
%
for ii = 3;
    to = [0,T(ii)];
    Xo = IC_L4(:,ii);
    [t,S] = ode113(@(t,S)CR3BP_n(t,S,u),to,Xo,options);
    S = S';
    p = plot3(S(1,:),S(2,:),S(3,:))

end

% Xo = [0.500033418907307
%     0.866003284749307
%     0
%     0.000123578139922591
%     -7.14171693615820e-05
%     -0.0170065910592695];
Xo = [0.499996997495189 
    0.866025403784439
    0
    0.000123578139922591
    -7.14171693615820e-05
    -0.0170065910592695];
T = [0,6.28318530886567/2];
[t,S] = ode113(@(t,S)CR3BP_n(t,S,u),T,Xo,options);
S = S';
% plot3(S(1,:),S(2,:),S(3,:));

J_target = jacobiConst(Xo(1:3),Xo(4:6),u);
% fmincon target =
% 1. position
% 2. vx and vy should be same,
% 3. vz(1) == -vz(end)
Xo_position = Xo(1:3);
vy_range = linspace(-7.14171693615820e-05,-1e-3,100);
Xi = [0.0432616375166833
-0.0249768693124458
-0.312110131029867;...
      6.28318617576206/2]; %vx and vz
J_target_list = linspace(2.9,3.1,3);
for ii = 1:length(J_target_list)
    [Xopt,fval,exitflag,output,lambda,grad,hessian] = fmincon(@CostFunction,...
        Xi,...     % Initial guess
        [], [],... % Linear inequality constraint
        [], [],... % Linear equality constraint
        [], [],... % Boundary constraint
        @(X)DynamicalConst(X,Xo_position,u,options,J_target_list(ii)),... % Nonlinear Constraint and options.
        options_fmincon);   % Options for fmincon
    if exitflag ~= 1
        break
    end
    x = Xo_position(1);
    y = Xo_position(2);
    z = Xo_position(3);
    vx = Xopt(1);
    vy = Xopt(2);
    vz = Xopt(3);
    T = Xopt(4);
    So = [x;y;z;vx;vy;vz];
    to = [0,T*2];
    [~,S] = ode113(@(t,S)CR3BP_n(t,S,u),to,So,options);
    S = S';
    p = plot3(S(1,:),S(2,:),S(3,:));
    IC_L4_VL(:,ii) = So;
    T_L4_VL(ii) = T*2;
    J_L4_VL(ii) = J_target_list(ii);
    pause(0.001);
    Xi = Xopt;
end
% IC_L4_VL=[IC_L4_VL,[IC_L4_VL(1:3,end);zeros(3,1)]];
% T_L4_VL = [T_L4_VL,T_L4_VL(end)];
% J_L4_VL= [J_L4_VL,3.001];
% save('SE_L4_Vertical.mat',"IC_L4_VL","T_L4_VL",'J_L4_VL');

function J = CostFunction(X)
J = 0;
end


function [c,ceq] = DynamicalConst(X,Xo_position,u,options,J_target)
x = Xo_position(1);
y = Xo_position(2);
z = Xo_position(3);
vx = X(1);
vy = X(2);
vz = X(3);
T = X(4);
So = [x;y;z;vx;vy;vz];
to = [0,T];
[~,S] = ode113(@(t,S)CR3BP_n(t,S,u),to,So,options);
%t = t';
S = S';
% plot3(S(1,:),S(2,:),S(3,:))
Jacobi = jacobiConst(So(1:3),So(4:6),u);
c = [];
ceq = [S(1:3,end)-Xo_position;X(1)-S(4,end);X(2)-S(5,end);X(3)+S(6,end);J_target-Jacobi];
 end


%%
% for ii = 1:length(z_target)
%     delete(p)
%     % Change x y position and x y z direction velocity with fixed z initial
%     % condition
%     [Xopt] = fmincon(@CostFunction,...
%         Xi,...     % Initial guess
%         [], [],... % Linear inequality constraint
%         [], [],... % Linear equality constraint
%         [], [],... % Boundary constraint
%         @(X)DynamicalConst(X,z_target(ii),u,options,J_target),... % Nonlinear Constraint and options.
%         options_fmincon);   % Options for fmincon
%
%     x = Xopt(1);
%     y = Xopt(2);
%     vx = Xopt(3);
%     vy = Xopt(4);
%     vz = Xopt(5);
%     T = Xopt(6);
%     So = [x;y;z_target(ii);vx;vy;vz];
%     to = [0,T];
%     [~,S] = ode113(@(t,S)CR3BP_n(t,S,u),to,So,options);
%     S = S';
%     p = plot3(S(1,:),S(2,:),S(3,:))
%     Xi = Xopt;
% %     delete(p)
% end
%  kind of good guess
% 0.500067167664022
% 0.865969703988816
% 5.94098606283545e-05
% -0.000133730359799367
% 0.0161752016025147
% 6.28318530886567

%%

% function J = CostFunction(X)
% J = 0;
% end
%
%
% function [c,ceq] = DynamicalConst(X,z_target,u,options,J_target)
% x = X(1);
% y = X(2);
% vx = X(3);
% vy = X(4);
% vz = X(5);
% T = X(6);
% So = [x;y;z_target;vx;vy;vz];
% to = [0,T];
% [~,S] = ode113(@(t,S)CR3BP_n(t,S,u),to,So,options);
% %t = t';
% S = S';
% % plot3(S(1,:),S(2,:),S(3,:))
% Jacobi = jacobiConst(So(1:3),So(4:6),u);
% c = [J_target-Jacobi,Jacobi-J_target];
% ceq = [So(1:3)-S(1:3,end),So(4:6)-S(4:6,end)]
% end

